import numpy as np
from scipy.stats import norm
import matplotlib.pyplot as plt

# 定义参数
phi = np.deg2rad(20) # 鱼雷初始航向与目标方位角之差，单位：弧度
beta = np.deg2rad(30) # 相对速度方向与目标方位角之差，单位：弧度
V = 15 # 鱼雷速度，单位：节
Vt = 50 # 目标速度，单位：节
X = np.deg2rad(40) # 鱼雷初始航向与相对速度方向之差，单位：弧度

# 定义误差
delta_D = 0.1 # 目标距离误差
delta_phi = np.deg2rad(1) # 鱼雷初始航向误差
delta_beta = np.deg2rad(1) # 相对速度方向与目标方位角误差
delta_V = 1 # 鱼雷速度误差
delta_Vt = 1 # 目标速度误差
delta_C = np.deg2rad(1) # 相对速度方向与鱼雷最终航向之差误差
delta_d_D = 0.1 # 目标距离误差可接受范围
delta_V_squared = 1 # 鱼雷速度误差方差
delta_phi_squared = np.deg2rad(1)**2 # 鱼雷初始航向误差方差

# 定义函数
def hit_probability(l, delta_D, delta_phi, delta_beta, delta_V, delta_Vt, delta_C, D):
    """
    计算在给定的误差范围内，鱼雷攻击目标所命中的概率。
    参数：
    l: 目标长度，单位：米
    delta_D: 目标距离误差，单位：米
    delta_phi: 鱼雷初始航向误差，单位：弧度
    delta_beta: 相对速度方向与目标方位角误差，单位：弧度
    delta_V: 鱼雷速度误差，单位：节
    delta_Vt: 目标速度误差，单位：节
    delta_C: 相对速度方向与鱼雷最终航向之差误差，单位：弧度
    D: 目标距离，单位：米
    返回：
    命中概率。
    """
    xq = (D - delta_D) * (np.sin(phi - delta_phi + delta_beta) - (V - delta_V) / (Vt - delta_Vt) * np.sin(X - delta_beta + delta_C)) / np.sin(X + phi - delta_phi + delta_C)
    sigma_xq_squared = ((D * np.cos(delta_beta) * delta_phi**2)**2
                        + ((D - delta_D) * np.cos(phi - delta_beta) * delta_phi)**2
                        + ((D - delta_D) * np.sin(phi - delta_beta) * delta_beta)**2
                        + (V / Vt**2 * np.sin(X - delta_beta + delta_C) * delta_V)**2
                        + ((D - delta_D) * np.cos(phi - delta_beta) / (Vt - delta_Vt) * np.sin(X - delta_beta + delta_C) * delta_Vt)**2
                        + ((D - delta_D) * np.sin(phi - delta_beta) * delta_C)**2
                        + (2 * np.cos(delta_beta) * delta_phi * (D**2 - 2 * D * delta_D + delta_D**2) * delta_beta)**2
                        + (2 * V / (Vt**2) * np.sin(phi - delta_beta) / np.sin(X + phi - delta_phi + delta_C) * delta_beta * delta_V)**2
                        + (2 * (D - delta_D) * V / (Vt * (Vt - delta_Vt)) * np.cos(phi - delta_beta) * np.sin(X - delta_beta + delta_C) * delta_beta * delta_Vt)**2
                        + (2 * (D - delta_D) * np.cos(phi - delta_beta) / (Vt - delta_Vt) * np.sin(X - delta_beta + delta_C) * delta_beta * delta_V)**2
                        + (2 * (D - delta_D) * np.sin(phi - delta_beta) / np.sin(X + phi - delta_phi + delta_C) * delta_beta * delta_C)**2
                        + (delta_d_D * D)**2
                        + delta_V_squared * (1 / (Vt - delta_Vt) * np.sin(X - delta_beta + delta_C))**2
                        + delta_phi_squared * (np.cos(delta_beta) * ((D - delta_D) * np.sin(phi - delta_beta) / np.sin(X + phi - delta_phi + delta_C))**2
                                             + np.sin(delta_beta) * ((D - delta_D) * np.cos(phi - delta_beta) / np.sin(X + phi - delta_phi + delta_C)
                                                                    - V / (Vt * np.sin(X + phi - delta_phi + delta_C)))**2))

    sigma_xq = np.sqrt(sigma_xq_squared)
    p = norm.cdf(l/2, loc=xq, scale=sigma_xq) - norm.cdf(-l/2, loc=xq, scale=sigma_xq)
    return p
# 计算命中概率并绘图
D = np.arange(100, 3001, 100) # 目标距离，单位：米
hit_prob = [hit_probability(l=100, delta_D=delta_D, delta_phi=delta_phi, delta_beta=delta_beta,
                            delta_V=delta_V, delta_Vt=delta_Vt, delta_C=delta_C, D=d) for d in D]
plt.plot(D, hit_prob)
plt.xlabel('Target distance (m)')
plt.ylabel('Hit probability')
plt.title('Hit probability vs. target distance')
plt.show()
